clear all;

% Discretization parameters
ModelSettings.SiteNum       = 100;       % Number of sites
ModelSettings.LengthUnit    = 'layer';   % 
ModelSettings.SiteWidth     = 2/sqrt(3); % The round particle

ModelSettings.x_max_EW      = pi;        % The x domain in the EW equation ranges from -pi to pi
ModelSettings.x_min_EW      = -pi;
ModelSettings.dx_EW = (ModelSettings.x_max_EW-ModelSettings.x_min_EW)/(2*ModelSettings.SiteNum);
dx_EW = ModelSettings.dx_EW;
x_EW = ModelSettings.x_min_EW:dx_EW:ModelSettings.x_max_EW;

ModelSettings.x_max         = ModelSettings.SiteNum*ModelSettings.SiteWidth;
ModelSettings.x_min         = 0;
ModelSettings.dx = (ModelSettings.x_max-ModelSettings.x_min)/(2*ModelSettings.SiteNum);
dx = ModelSettings.dx;
x = ModelSettings.x_min:dx:ModelSettings.x_max;

Mode = 100;
ModelSettings.mode          = Mode;

K_alpha = zeros(ModelSettings.mode,1);
K_beta  = zeros(ModelSettings.mode,1);
for n=1:ModelSettings.mode
    h_sin = sin(n*x_EW);
    dh_sin = zeros(size(h_sin));
    dh_sin(1) = h_sin(1)-h_sin(end);
    dh_sin(2:end) = diff(h_sin);
%     K_alpha(n) = sum(dh_sin.^2)/(2*ModelSettings.SiteNum*pi*dx_EW^2);
    K_alpha(n) = sum(dh_sin.^2)/(pi*dx^2);

    h_cos = cos(n*x_EW);
    dh_cos = zeros(size(h_cos));
    dh_cos(1) = h_cos(1)-h_cos(end);
    dh_cos(2:end) = diff(h_cos);
%     K_beta(n)  = sum(dh_cos.^2)/(2*ModelSettings.SiteNum*pi*dx_EW^2); 
    K_beta(n)  = sum(dh_cos.^2)/(pi*dx^2);  
end
ModelSettings.M2ModeWeighting=[K_alpha,K_beta];
% ModelSettings.M2ModeWeighting=[K_alpha,K_beta]*((dx_EW/dx)^2)*2*ModelSettings.SiteNum;

n = 1:Mode;
L = ModelSettings.SiteNum;
K_alpha_ana = 4/(2*L*dx_EW^3)*(sin(n*pi/(2*L)).^2);
K_beta_ana = K_alpha_ana;

% ModelSettings.M2ModeWeighting=[K_alpha_ana',K_beta_ana']*((dx_EW/dx)^2)*2*ModelSettings.SiteNum;

figure(1);
subplot(2,1,1);
plot(n,K_alpha,n,K_alpha_ana,'o');
xlabel('Mode');
ylabel('K_{alpha}');
subplot(2,1,2);
plot(n,K_beta,n,K_beta_ana,'o');
xlabel('Mode');
ylabel('K_{beta}');

% Parameters about the model
% Assuming the data from KMC is in a domain [-L,L]
ModelSettings.MV      = 'T';
ModelSettings.FitTo   = 'meanR2';
ModelSettings.FittingTimeRange = 1000;
% ModelSettings.W       = [0.10,0.15,0.20,0.25,0.30,0.35,0.40,0.45,0.50];
ModelSettings.T       = [300,400,450,500,550,600,620,630,640,650,670,700];
ModelSettings.nu      = [2.6068e-003,2.5277e-003,2.4379e-003,1.0669e-003,0.4869e-003,0.2657e-003,9.6627e-005,8.0198e-006,0.8427e-003,6.6350e-003,1.855e-002,4.8547e-002];
ModelSettings.sigma2  = [0.6307,0.6179,0.6334,0.4737,0.3096,0.1757,0.0291,0.0028,0.0131,0.0395,0.0638,0.0971];
ModelSettings.Rh      = [0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2 0.2];
ModelSettings.K       = [0.6440 0.6439 0.6520 0.7676 0.9138 0.9823 0.9876 0.9902 0.9909 0.9916 0.9930 0.9939];
ModelSettings.Tau     = [3.9399 3.9226 4.0073 4.6479 3.5363 2.9746 2.7827 2.6868 2.6295 2.5721 2.4575 2.3667];

ModelSettings.h = 0;
ModelSettings.rho = 0;
ModelSettings.meanAlpha2 = zeros(ModelSettings.mode,1);
ModelSettings.meanBeta2  = zeros(ModelSettings.mode,1);
ModelSettings.varAlpha2  = zeros(ModelSettings.mode,1);
ModelSettings.varBeta2   = zeros(ModelSettings.mode,1);
ModelSettings.meanR2     = model_CalMeanR2(ModelSettings);
ModelSettings.meanM2     = model_CalMeanM2(ModelSettings);

%%
% MPCsettings = MPC1xx_ini('model_settings1.mat');
MPCsettings.D_set   = 0.98;
MPCsettings.Fact_D  = 0.0;
MPCsettings.R2_set  = 40;
MPCsettings.Fact_r2 = 0.0;
MPCsettings.Ht_set  = 300;
MPCsettings.Fact_H  = 0.0;
MPCsettings.Fact_varR2 = 0.0;
MPCsettings.M2_set  = 0.5;
MPCsettings.M2_fact = 1.0;
MPCsettings.P       = 5;
% MPCsettings.m       = 20;
MPCsettings.dt      = 5;
MPCsettings.ub      = 900;
MPCsettings.lb      = 300;
MPCsettings.rt_T    = 10;
MPCsettings.ControllerTypeID = 104;
MPCsettings.model = ModelSettings;
MPCsettings.costfun = @objfun;
MPCsettings.nonlcon = @nonlcon105;
MPCsettings.weight = [1.0,0.8,0.6,0.4,0.2];
MPCsettings.Input = {'alpha','beta','h','rho'};
MPCsettings.Ouput = {'T(K)'};
MPCsettings.model = ModelSettings;
%%
feedback.t     = 10;
feedback.rho   = 0.98;
feedback.h     = 300;
feedback.alpha = randn(MPCsettings.model.mode,1)./((1:MPCsettings.model.mode).^2)';
feedback.beta  = randn(MPCsettings.model.mode,1)./((1:MPCsettings.model.mode).^2)';

%% Compare calculation of meanM2 from mode and from h profile.
% ModelSettings.M2ModeWeighting(end,1) = 0;
% ModelSettings.M2ModeWeighting(end,2) = 0;
ModelSettings = model_calstat(ModelSettings,feedback);
fprintf(1,'From mode, M2 = %f\n',ModelSettings.meanM2);

x_KMC = ModelSettings.x_min:dx:ModelSettings.x_max;
x_EW  = ModelSettings.x_min_EW:dx_EW:ModelSettings.x_max_EW;
h = sum(repmat(feedback.alpha,1,length(x_EW)).*sin((1:ModelSettings.mode)'*x_EW)+repmat(feedback.beta,1,length(x_EW)).*cos((1:ModelSettings.mode)'*x_EW))/sqrt(pi);
meanM2 = sum(diff([h(end),h]).^2*3)/(2*ModelSettings.SiteNum);
fprintf(1,'From h profile directly, M2 = %f\n',meanM2);

alphas = zeros(ModelSettings.mode,1);
betas = zeros(ModelSettings.mode,1);
for i = 1:ModelSettings.mode
    alphas(i) = 0;
    betas(i) = 0;
    for j = 1:2*ModelSettings.SiteNum
        alphas(i) = alphas(i) + h(j)*sin(i*x_EW(j))/sqrt(pi);
        betas(i) = betas(i)+h(j)*cos(i*x_EW(j))/sqrt(pi);
    end
    alphas(i) = alphas(i)*dx_EW;
    betas(i) = betas(i)*dx_EW;
end
%%
figure(1);plot(1:ModelSettings.mode,feedback.alpha,'o',1:ModelSettings.mode,alphas,'+');title('alpha');
figure(2);plot(1:ModelSettings.mode,feedback.beta,'o',1:ModelSettings.mode,betas,'+');title('beta');

%%
objfun_T([500 500 500 500 500],feedback,MPCsettings)
%%
[x,info,MPCsettings] = MPC_Matlab_T_update(MPCsettings,feedback,500)

%% Case #2
% MPCsettings.D_set   = 0.98;
% MPCsettings.Fact_D  = 1.0;
% MPCsettings.R2_set  = 40;
% MPCsettings.Fact_r2 = 0.0;
% MPCsettings.Ht_set  = 300;
% MPCsettings.Fact_H  = 0.0;
% MPCsettings.Fact_varR2 = 0.0;
% MPCsettings.M2_set  = 0.5;
% MPCsettings.M2_fact = 1.0;
% 
% [x,info] = MPC_Matlab_b(MPCsettings,feedback,500)
